#include "MBOctree.h"
#include "MBTriangleSightFrustum.h"
#include "MBTriangleSightTriangle.h"
#include "MBTriangle.h"

#include <iostream>
#include <set>
#include <vector>

using std::cout;
using std::endl;
using std::set;
using std::vector;

const MBuint X = 0u;
const MBuint Y = 1u;
const MBuint Z = 2u;

/**
  * Constructs a new empty Octree object
  */
MBOctree::MBOctree( void )
{
	mRoot = 0;
}

/**
  * Destroys the MBOctree object and all the MBOctreeNode objects inside it
  */
MBOctree::~MBOctree()
{
	if (mRoot) delete mRoot;
}

/**
  * Returns the number of MBTriangle indices stored in the Octree
  * @return number of MBTriangle indices stored in the Octree
  */
MBuint MBOctree::GetNumberOfTriangles( void ) const
{
	return mRoot->GetNumberOfTriangles();
}

/**
  *	Initializes the Octree with the given arguments
  *
  * @param centerPosition center position of the Octree
  * @param sideLengths side lengths of the Octree in x, y, and z direction
  * @param maxLevels maximum number of levels
  * @param triangles const pointer to the vector with pointers to all MBTriangle objects that are inserted into the Octree
  * @param vertexCoordinates const pointer to the vector with all the 3D vertex positions, stored as pointers to double arrays
  */
void MBOctree::Init(
	const MBVec3 &centerPosition,
	const MBVec3 &sideLengths,
	const MBuint &maxLevels,
	const std::vector<MBTriangle*> *triangles,
	const std::vector<double*> *vertexCoordinates )
{
	mTriangles = triangles;
	mVertexCoordinates = vertexCoordinates;

	MBVec3 min = centerPosition - (sideLengths * 0.5);
	MBVec3 max = centerPosition + (sideLengths * 0.5);

	mRoot = new MBOctreeNode( min, max, 1, maxLevels );

	const MBTriangle *triangle = 0;
	const MBuint *t_v_i = 0;

	for (MBuint i = 0; i < triangles->size(); i++)
	{
		triangle = (*triangles)[i];
		t_v_i = triangle->GetVertexIndices();
		const vector<double*> &v = *vertexCoordinates;

		const MBVec3 triangle_points[3] = {
			MBVec3( v[t_v_i[0]][X], v[t_v_i[0]][Y], v[t_v_i[0]][Z] ),
			MBVec3( v[t_v_i[1]][X], v[t_v_i[1]][Y], v[t_v_i[1]][Z] ),
			MBVec3( v[t_v_i[2]][X], v[t_v_i[2]][Y], v[t_v_i[2]][Z] ) };

			mRoot->AddTriangle( i, triangle_points );
	}
}

/**
  *	Returns true if any triangle in the Octree is intersecting with the given
  * sight frustum, false otherwise.
  *
  * @param triangleVertexIndices pointer to the three vertex positions of the triangle
  * @param cameraPosition position of the camera
  * @return true if any triangle in the Octree is intersecting with the given sight frustum, false otherwise.
  */
bool MBOctree::IsOtherTriangleIntersectingSightFrustum(
	const MBuint *triangleVertexIndices,
	const MBVec3 &cameraPosition ) const
{
	const MBuint *t_v_i = triangleVertexIndices;
	const vector<double*> &v = *mVertexCoordinates;

	const MBVec3 triangle_points[3] = {
		MBVec3( v[t_v_i[0]][X], v[t_v_i[0]][Y], v[t_v_i[0]][Z] ),
		MBVec3( v[t_v_i[1]][X], v[t_v_i[1]][Y], v[t_v_i[1]][Z] ),
		MBVec3( v[t_v_i[2]][X], v[t_v_i[2]][Y], v[t_v_i[2]][Z] ) };

	MBTriangleSightFrustum sight_frustum( triangle_points, cameraPosition );

	//set<MBuint> already_checked_triangle_indices;

	return mRoot->IsOtherTriangleIntersectingSightFrustum(
		sight_frustum,
		triangleVertexIndices,
		mTriangles,
		mVertexCoordinates );
		//already_checked_triangle_indices );
}

/**
  *	Returns true if any triangle in the Octree is intersecting with the given
  * sight triangle, false otherwise.
  *
  * @param edgeVertexIndices pointer to the two vertex positions of the edge
  * @param cameraPosition position of the camera
  * @return true if any triangle in the Octree is intersecting with the given sight triangle, false otherwise.
  */
bool MBOctree::IsOtherTriangleIntersectingSightTriangle(
	const MBuint *edgeVertexIndices,
	const MBVec3 &cameraPosition ) const
{
	const MBuint *t_v_i = edgeVertexIndices;
	const vector<double*> &v = *mVertexCoordinates;

	const MBVec3 edge_points[2] = {
		MBVec3( v[t_v_i[0]][X], v[t_v_i[0]][Y], v[t_v_i[0]][Z] ),
		MBVec3( v[t_v_i[1]][X], v[t_v_i[1]][Y], v[t_v_i[1]][Z] ) };

	MBTriangleSightTriangle sight_triangle( edge_points, cameraPosition );

	return mRoot->IsOtherTriangleIntersectingSightTriangle(
		sight_triangle,
		edgeVertexIndices,
		mTriangles,
		mVertexCoordinates );
}